/*
#include <Motion.h>
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-11-28     Administrator       the first version
 */


#include "ActionQueue.h"
#include <rtthread.h>
#include <stdlib.h>
#include <iostream>
#include <cstring>
#include "ulog.h"

#include "GlobalVar.h"
#include "DevManager.h"

/**
简易通讯协议

第1字节    第2字节    第3字节    第4字节    第5字节    第6字节    第7字节    第8字节    总字节数    空闲字节数
1字节         1字节                                               0-5字节                                       1字节
                    操作模式                                        参数                                              校验
设备号       0速度点位     速度1字节   点位1字节   CRC8校验1字节               5   3
            1速度相对位置 速度1字节   相对位置4字节   CRC8校验1字节   8   0
            2速度  速度1字节   CRC8校验1字节                   4   4
            3开关  开关1字节   CRC8校验1字节                   4   4
            4延时   时间4字节                  CRC8校验1字节   8   0
            5模拟传感器  阈值4字节               CRC8校验1字节       5   3
            6液面探测   次数1字节   下探深度4字节             CRC8校验1字节   8   0
            7二值传感器  开关1字节   跳转步数1字节 CRC8校验1字节               5   3
            8复位     CRC8校验1字节                       3   5
*/



ActionQueue ActQue_0(0);
ActionQueue ActQue_1(1);
ActionQueue ActQue_2(2);
ActionQueue ActQue_3(3);
ActionQueue ActQue_4(4);
ActionQueue ActQue_5(5);
ActionQueue ActQue_6(6);
ActionQueue ActQue_7(7);

ActionQueue* ActQueTask[65]=
{
        &ActQue_0,&ActQue_1,&ActQue_2,
        &ActQue_3,&ActQue_4,&ActQue_5,
        &ActQue_6,&ActQue_7
};

/*  CRC  */
int CRC8(int *buf,int lenth)
{
    return 1;
}

/*  Constructor  */
ActionQueue::ActionQueue(int _action_queue_num){action_queue_num = _action_queue_num;}
ActionQueue::~ActionQueue(){}

/*  If the board have motor function ,please define
 * MOTOR_FUNC macro
 */

void ActionQueue::AddTaskToMyDev(uint8_t devNo)
{


}

//static void MotorDirverStartUp(rt_uint8_t dirveNnum,ACT_INFO ActInfo)
//{
//    /* 获取一下模块位置 */
//    rt_uint32_t tagetPos = DummyMotorPtr[dirveNnum]->GetPosInfo(ActInfo.posNo);
//    /* 设置目标位置 */
//    tmc_driver[dirveNnum]->SetTargetPos(tagetPos);
//    /* 发送一个信号量启动电机 */
//    //rt_sem_release(sem_motor[dirveNnum].RunStart);
//}

//void ActionQueue::StartPosMode(rt_uint8_t devNo,ACT_INFO ActInfo)
//{
//    /* Retrieve the hardware link for the device */
//    rt_uint8_t dirver_num = MotorDiverNum(devNo);
//    /* 启动对应的驱动器 */
//    MotorDirverStartUp(dirver_num,ActInfo);
//}



/* When the device is not my board device,just send the message
 * to anther board.*/
uint8_t ActionQueue::SendTaskToTargetBoard(uint8_t devNo)
{
    /* Query of device board num.*/
    uint8_t target_board_num = QueryOfDevBoardNum(devNo);

    uint32_t ExtId = 0;

/* message CAN ExtID
 *   SourceAddr 4
 *   TargetAddr 4
 *   Protocol type 4
 *   dataLenth 4
 *   reserve 13 */

    ExtId |= (machine_info.address &= 0x0f);
    ExtId <<= 4;
    ExtId |= (target_board_num &= 0x0f);
    ExtId <<= 4;
    ExtId |=  0x01;
    ExtId <<= 4;

    //CAN send
    CAN_Send(curTask,8,ExtId);

    /* Define the retransmission mechanism */
//    rt_err_t ret;
//    int Timeout_times = 10;
//    int Timeout_time = 50;
    rt_ubase_t value = 0;
    //sourceAddr = 4 byte targetAddr = 4
//    ret = rt_mb_recv(Mailbox_AQ[ActQueNo], &value, Timeout_time) ;
//
//    while(Timeout_times != 0 && ret == -RT_ETIMEOUT)
//    {
//        Timeout_times--;
//        CAN_Send(curTask,8,ExtId);
//        ret = rt_mb_recv(Mailbox_AQ[ActQueNo], &value, Timeout_time);
//    }

    if (value != SLAVE_RECEIVE_TASK) {
        LOG_E("Target board %d numbered M_%d named %s "
                "have no answer,please check the hard-"
                "ware link.",
                target_board_num,
                devNo,
                dev_info[devNo].dev_name);
    }
    return true;
}

void ActionQueue::WaitTaskMsgAnswer(uint8_t devNo)
{
    //rt_err_t ret;
    rt_ubase_t value;
    //ret = rt_mb_recv(Mailbox_AQ[ActQueNo], &value, 30000);

   // if(ret == -RT_ETIMEOUT)
    {
        LOG_E("Device act out-time.");
        while(1)
        {
            DelayMs(65535);
        }
    }

    if(value != SLAVE_COMPLETE_TASK)
    {
        LOG_E("Device act wrong.");
        while(1)
        {
            DelayMs(65535);
        }
    }

}

void ActionQueue::CAN_Send(uint8_t *p,uint8_t dataLenth,uint32_t ExtID)
{
    //向CAN发送队列中添加一个待发送数据
    //rt_mq_send(mq, buffer, size);
    uint buf[12];
    buf[3] = ExtID;
    buf[2] = ((ExtID>>4)&0x0F);
    buf[1] = ((ExtID>>8)&0x0F);
    buf[0] = ((ExtID>>12)&0x0F);
    memcpy(buf+4,p,dataLenth);
    //rt_mq_send(can1_send_mq, buf, sizeof(buf));
}

void ActionQueue::SetActQueTotalStep(uint8_t _total_step)
{
    total_step = _total_step;
}


void ActionQueue::Process(void *parameter)
{
    /**等待一个信号量开启进程 */
    uint8_t Task[8] = {0};
    uint8_t devNo = 0;
    uint8_t driver_num = 0;
    uint8_t devType = 0;
    uint32_t time_ms = 0;
    rt_err_t ret;

    rt_sem_take(task.action_queue[action_queue_num].start_up, RT_WAITING_FOREVER);
    LOG_I("actiong queue num %d is running.",action_queue_num);
    remaining_step = total_step;
    current_step = 1;

    for (int i = 0; i < 20; i++)
    {
        //uint buffer[8];
        //调用FLASH读取函数读取一包动作包
        //发送一包动作包
        //rt_mq_send(mq_aq_task[ActQueNo], buffer, 8);
    }

    //rt_sem_release(mq_aq_flash_read[ActQueNo]);
    while(remaining_step != 0)
    {
        rt_mq_recv(task.action_queue[action_queue_num].task_msg_queue, Task, 8, RT_WAITING_FOREVER);

        LOG_I("Toltal step is %d,current step is %d,remaining step is %d",
                total_step,
                current_step,
                remaining_step);

        devNo = Task[0];
        driver_num = QueryOfDevDriverNum(devNo);
        devType = QueryOfDevType(devNo);
        //rt_mutex_take(mux_dev[devNo], RT_WAITING_FOREVER);
        if(MyBoardDev(devNo) == true)
        {
            switch (devType) {
            case MOTOR:
                rt_mq_send(task.motor_driver[driver_num].task_msg_queue, Task, 8);
                rt_sem_take(task.motor_driver[driver_num].complete, RT_WAITING_FOREVER);
                break;
            case IO:
                rt_mq_send(task.IO_driver.task_msg_queue, Task, 8);
                break;
            case DELAY:
                Byte_to_Uint32t(&time_ms, &Task[2]);
                rt_thread_mdelay(time_ms);
                break;
            default:
                break;
            }
        }
        else
        {
            uint8_t target_board_num = QueryOfDevBoardNum(devNo);
            uint32_t ExtId = 0;
            ExtId |= (machine_info.address &= 0x0f);
            ExtId <<= 4;
            ExtId |= (target_board_num &= 0x0f);
            ExtId <<= 4;
            ExtId |=  0x01;
            ExtId <<= 4;
            CAN_Send(Task,8,ExtId);
            ret = rt_mq_recv(task.action_queue[driver_num].can_msg_queue, Task, 8, 10000);
            if(ret == -RT_ETIMEOUT)
            {
                LOG_E("device %d act outtime.",devNo);
            }
        }
        //rt_mutex_release(mux_dev[devNo]);
        LOG_I("Action queue num %d action num %d have done.",action_queue_num,current_step);

        remaining_step--;
        current_step++;
    }
    /** Release a completion semaphore*/
    rt_sem_release(task.action_queue[action_queue_num].complete);
}



void (*Action_Queue_Task_Ptr[21]) (void *parameter) =
{
        AQ0_Task,
        AQ1_Task,
        AQ2_Task,
        AQ3_Task,
        AQ4_Task,
        AQ5_Task,
        AQ6_Task,
        AQ7_Task,
        AQ8_Task,
        AQ9_Task,
        AQ10_Task,
        AQ11_Task,
        AQ12_Task,
        AQ13_Task,
        AQ14_Task,
        AQ15_Task,
        AQ16_Task,
        AQ17_Task,
        AQ18_Task,
        AQ19_Task,
        AQ20_Task,
};

void AQ0_Task(void *parameter){while(1){ActQueTask[0]->Process(parameter);}}
void AQ1_Task(void *parameter){while(1){ActQueTask[1]->Process(parameter);}}
void AQ2_Task(void *parameter){while(1){ActQueTask[2]->Process(parameter);}}
void AQ3_Task(void *parameter){while(1){ActQueTask[3]->Process(parameter);}}
void AQ4_Task(void *parameter){while(1){ActQueTask[4]->Process(parameter);}}
void AQ5_Task(void *parameter){while(1){ActQueTask[5]->Process(parameter);}}
void AQ6_Task(void *parameter){while(1){ActQueTask[6]->Process(parameter);}}
void AQ7_Task(void *parameter){while(1){ActQueTask[7]->Process(parameter);}}
void AQ8_Task(void *parameter){while(1){ActQueTask[8]->Process(parameter);}}
void AQ9_Task(void *parameter){while(1){ActQueTask[9]->Process(parameter);}}
void AQ10_Task(void *parameter){while(1){ActQueTask[10]->Process(parameter);}}
void AQ11_Task(void *parameter){while(1){ActQueTask[11]->Process(parameter);}}
void AQ12_Task(void *parameter){while(1){ActQueTask[12]->Process(parameter);}}
void AQ13_Task(void *parameter){while(1){ActQueTask[13]->Process(parameter);}}
void AQ14_Task(void *parameter){while(1){ActQueTask[14]->Process(parameter);}}
void AQ15_Task(void *parameter){while(1){ActQueTask[15]->Process(parameter);}}
void AQ16_Task(void *parameter){while(1){ActQueTask[16]->Process(parameter);}}
void AQ17_Task(void *parameter){while(1){ActQueTask[17]->Process(parameter);}}
void AQ18_Task(void *parameter){while(1){ActQueTask[18]->Process(parameter);}}
void AQ19_Task(void *parameter){while(1){ActQueTask[19]->Process(parameter);}}
void AQ20_Task(void *parameter){while(1){ActQueTask[20]->Process(parameter);}}


void test_aq()
{
    uint8_t buffer[8];
    buffer[0] = 5;
    buffer[1] = 4;
    buffer[2] = 1;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 6;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 0;
    buffer[1] = 4;
    Uint32t_to_Byte(1000, &buffer[2]);
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);

    buffer[0] = 7;
    buffer[1] = 4;
    buffer[2] = 1;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 8;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 9;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 10;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 11;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);
    buffer[0] = 12;
    rt_mq_send(task.action_queue[1].task_msg_queue, buffer, 8);

    ActQue_1.SetActQueTotalStep(8);
    /** 开启时序1 */
    rt_sem_release(task.action_queue[1].start_up);
}

#ifdef FINSH_USING_MSH
MSH_CMD_EXPORT(test_aq, a demo aq sample);
#endif



